clear;
clc;
%%%%%%%%%%%%%%%%%%%%%%建立环境矩阵map及参数初始化%%%%%%%%%%%%%%%%%%%%%
map = zeros(22,32);
map(4:5,1) = 1;
map(8,1) = 1;
map(11:12,1) = 1;
map(18:19,1) = 1;
map(1,2) = 1;
map(1,8:10) = 1;
map(1,20:22) = 1;
map(6:7,6:15) = 1;
map(6:7,18:27) = 1;
map(9:10,6:15) = 1;
map(9:10,18:27) = 1;
map(13:14,6:15) = 1;
map(13:14,18:27) = 1;
map(16:17,6:15) = 1;
map(16:17,18:27) = 1;
map(22,8:10) = 1;
map(22,20:22) = 1;
map(21,32) = 1;
map(7:9,32) = 1;
map(15:17,32) = 1;

global SE;
% SE = [447,244;244,223;222,671;671,363;363,660;660,671;671,567;567,662;662,671;671,556;556,650;650,671;
%     671,150;150,54;54,34;34,239;239,52;52,34];   %,优先级顺序任务序列（任意数量）
% SE = [447,244;244,223;222,671;671,363;363,368;368,400;400,656;656,660;660,671];
SE = [447,54;346,196;509,633;197,45];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
          %以上是参数可更改区域，以下所有功能函数已封装%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[W,C] = MapInit(map);                                              %环境初始化
[PrePathSet,PreTimeWindow,PrePathIndex] = Preplanned_Path(map,W);  %路径预规划
planningPath(map,PrePathSet,PreTimeWindow,PrePathIndex);           %规划路径
plotTW(map,C);                                                     %画出时间窗
plotMap_Path(map,C);                                               %得到环境地图 
